/**
 * @file Robot.cpp
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief 机器人类
 */

#include "Robot.h"
#include <cstdio>
#include "def.h"
#include "utils.h"
#include "Params.h"

const double Robot::EMPTY_RADIUS = 0.45; // 空载半径
const double Robot::FULL_RADIUS = 0.53; // 满载半径

/**
 * @brief 初始化
 * @param id 机器人ID
 * @param x 横坐标格号
 * @param y 纵坐标格号
 */
void Robot::init(int id, int x, int y) {
    this->id = id;
    this->set(x, y);
}

/**
 * @brief 数据更新
 * @param benchID 所处工作台ID
 * @param materialType 携带物品类型
 * @param timeFactor 时间价值系数
 * @param crashFactor 碰撞价值系数
 * @param angleSpeed 角速度
 * @param lineSpeed_x x轴线速度
 * @param lineSpeed_y y轴线速度
 * @param yaw 当前朝向
 * @param coord_x 横坐标
 * @param coord_y 纵坐标
 */
void Robot::refresh(int benchID, int materialType, double timeFactor, double crashFactor, double angleSpeed,
                    double lineSpeed_x, double lineSpeed_y, double yaw, double coord_x, double coord_y) {
    this->benchID = benchID;
    this->materialType = materialType;
    this->timeFactor = timeFactor;
    this->crashFactor = crashFactor;
    this->angleSpeed = angleSpeed;
    this->lineSpeed = Vector(lineSpeed_x, lineSpeed_y);
    this->yaw = yaw;
    this->x = coord_x;
    this->y = coord_y;
}

/**
 * @brief 初始化PID参数
 * @param forwardEmpty 空载时前进PID参数
 * @param forwardFull 满载时前进PID参数
 * @param rotateEmpty 空载时旋转PID参数
 * @param rotateFull 满载时旋转PID参数
 */
void Robot::initPID(PIDParam_t forwardEmpty, PIDParam_t forwardFull, PIDParam_t rotateEmpty, PIDParam_t rotateFull) {
    this->forwardEmpty = forwardEmpty;
    this->forwardFull = forwardFull;
    this->rotateEmpty = rotateEmpty;
    this->rotateFull = rotateFull;
    this->forwardPID.setParam(this->forwardEmpty);
    this->rotatePID.setParam(this->rotateEmpty);
}

/**
 * @brief 前进控制
 * @param speed 前进速度
 */
void Robot::forward(double speed) {
    std::printf("forward %d %f\n", this->id, speed);
    std::fflush(stdout);
}

/**
 * @brief 旋转控制
 * @param speed 旋转速度
 */
void Robot::rotate(double speed) {
    std::printf("rotate %d %f\n", this->id, speed);
    std::fflush(stdout);
}

/**
 * @brief 购买
 */
void Robot::buy() {
    std::printf("buy %d\n", this->id);
    std::fflush(stdout);
    this->radius = FULL_RADIUS;
    this->forwardPID.setParam(this->forwardFull);
    this->rotatePID.setParam(this->rotateFull);
}

/**
 * @brief 出售
 */
void Robot::sell() {
    std::printf("sell %d\n", this->id);
    std::fflush(stdout);
    this->radius = EMPTY_RADIUS;
    this->forwardPID.setParam(this->forwardEmpty);
    this->rotatePID.setParam(this->rotateEmpty);
}

/**
 * @brief 销毁已携带的物品
 */
void Robot::destroy() {
    std::printf("destroy %d\n", this->id);
    std::fflush(stdout);
    this->radius = EMPTY_RADIUS;
    this->forwardPID.setParam(this->forwardEmpty);
    this->rotatePID.setParam(this->rotateEmpty);
}

/**
 * @brief 获取半径
 * @return double 半径
 */
double Robot::getRadius() {
    return this->radius;
}

/**
 * @brief 转向目标坐标
 * @param target 目标坐标
 */
void Robot::turnTo(Coord target) {
    this->forwardPID.stop();
    this->coordTarget = target;
    this->rotatePID.start();
    this->isCtrling = true;
}

/**
 * @brief 转向目标坐标
 * @param x 目标横坐标
 * @param y 目标纵坐标
 */
void Robot::turnTo(double x, double y) {
    this->turnTo(Coord(x, y));
}

/**
 * @brief 移动到目标坐标
 * @param target 目标坐标
 * @param needWaitRotate 是否需要等待旋转控制完成再起跑
 */
void Robot::runTo(Coord target, bool needWaitRotate) {
    this->needWaitRotate = needWaitRotate;
    this->turnTo(target);
    this->isRun = true;
    this->forwardPID.start();
}

/**
 * @brief 移动到目标坐标
 * @param x 目标横坐标
 * @param y 目标纵坐标
 * @param needWaitRotate 是否需要等待旋转控制完成再起跑
 */
void Robot::runTo(double x, double y, bool needWaitRotate) {
    this->runTo(Coord(x, y), needWaitRotate);
}

/**
 * @brief 获得旋转控制量
 * @return double 旋转控制量
 */
double Robot::getRotate() {
    double towardTarget = utils::angleConvert(this->toward(this->coordTarget, this->yaw));
    double output = this->rotatePID.getOutput(towardTarget, this->yaw, ROBOT_ROTATE_MAX);
    if(!this->isRun) { // 如果不是移动任务，达到目标精度就停下来
        if(this->rotatePID.isFinish()) {
            this->rotatePID.stop();
            this->isCtrling = false;
        }
    }
    return output;
}

/**
 * @brief 停止旋转控制
 */
void Robot::stopRotateCtrl() {
    this->rotatePID.stop();
}

/**
 * @brief 停止控制
 */
void Robot::stopCtrl() {
    this->stopRotateCtrl();
    this->forwardPID.stop();
    this->isRun = false;
    this->isCtrling = false;
}

/**
 * @brief 获得前进控制量
 * @param minForward 最小前进速度
 * @param maxForward 最大前进速度
 * @return double 前进控制量
 */
double Robot::getForward(double minForward, double maxForward) {
    double output = this->forwardPID.getOutput(0, this->distance(this->coordTarget), minForward, maxForward);
    if(this->isRun) { // 如果是移动任务，达到目标距离后停下旋转和前进控制
        if(this->forwardPID.isFinish()) {
            this->stopCtrl();
        }
    }
    return output;
}

/**
 * @brief 判断是否完成控制任务
 * @return true 已经完成控制任务
 * @return false 未完成控制任务
 */
bool Robot::isFinishedCtrl() {
    return !this->isCtrling;
}

/**
 * @brief 发送控制指令
 */
void Robot::sendCtrlCommand() {
    if(this->isCtrling) {
        this->rotate(-this->getRotate());
        if(this->rotatePID.isFinish()) {
            this->needWaitRotate = false;
            this->forward(this->getForward());
        } else if(this->rotatePID.isFinish(PARAMS.ROTATE_CAN_RUN_PRECISION) && !this->needWaitRotate) {
            this->forward(this->getForward(ROBOT_FORWARD_MIN, this->isEmpty() ? PARAMS.WAIT_ROTATE_SPEED_EMPTY : PARAMS.WAIT_ROTATE_SPEED_FULL));
        } else {
            this->forward(0);
        }
    } else {
        this->rotate(0);
        this->forward(0);
    }
}

/**
 * @brief 获得携带货物类型
 * @return int 携带物品类型
 */
int Robot::getMaterialType() {
    return this->materialType;
}

/**
 * @brief 获取机器人ID
 * @return int 机器人ID
 */
int Robot::getID() {
    return this->id;
}

/**
 * @brief 查询机器人是否为空载状态
 * @return true 空载状态
 * @return false 满载状态
 */
bool Robot::isEmpty() {
    return this->getMaterialType() == 0;
}

/**
 * @brief 获取机器人所处工作台ID
 * @return int 机器人所处工作台ID(-1为不在工作台附近)
 */
int Robot::getBenchID() {
    return this->benchID;
}

/**
 * @brief 向所有机器人发送速度清零命令
 */
void Robot::pauseAll() {
    for(int id = 0; id < ROBOT_NUM_MAX; ++id) {
        std::printf("forward %d 0.0\n", id);
        std::printf("rotate %d 0.0\n", id);
    }
    std::fflush(stdout);
}
